#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>

#include "facedir.h"

#include "other_functions.h"

#include "view.h"
#include "model.h"
#include "createmodel.h"
#include "cvfundam2.h"

void on_mouse0(int event, int x, int y, int flags, void* param);
void on_mouse1(int event, int x, int y, int flags, void* param);

void CFacedir::createModel_Manual()
{
    CvSize size = reader.image_size();
    creation = new CCreateModel(size, view[0]);

    cvNamedWindow(window[0], 1);
    cvMoveWindow(window[0], 0,0);
    cvNamedWindow(window[1], 1);
    cvMoveWindow(window[1], 0,size.height+20);
    cvNamedWindow(Model_SMAT[0], 1);
    cvMoveWindow(Model_SMAT[0], size.width+10,0);
    
    cvComputeCorrespondEpilines(view[0].points2d, 1, view[0].calibration->Fundamental, model.epilines);
    
    view[0].calcPose(view[0].mask_show);
    view[1].ChangeView(view[0]);

    creation->modelview = view[0];
    creation->modelview.resetR();
    //creation->modelview.setTx();
    //creation->modelview.setTy();

    cvSetMouseCallback(window[0], on_mouse0, (void*)this);    
    cvSetMouseCallback(window[1], on_mouse1, (void*)this);    
    
    view[0].setmask_show();
    view[1].setmask_show();
    creation->modelview.setmask_show();
    MostrarModelo();

    model.calcLinesModel();

    //centra el modelo sobre el punto 0 para un correcto uso de cvPOSIT / cvProjectPoints2
    model.CentreModel();    
    
    cvSetMouseCallback(window[0], NULL, NULL);
    cvSetMouseCallback(window[1], NULL, NULL);	

    delete creation;
}



void CFacedir::MostrarModelo()
{
    int key = 0, key2=0;
    float increment2 = 6.0F*CV_PI/180.0F;
    float increment=0.5/180.0*CV_PI;
    float incrementpos = 1;
    double ang, difang ;
    int hide_points=0;
    int draw = 1;

    do 
    {
	
	if (draw)
	{
	    //draw the model in white background
	    if (hide_points)
	    {
		creation->modelview.getNotHiden(model.angY);
		view[0].getNotHiden();
		view[1].getNotHiden();

	    }
	    else
	    {
		creation->modelview.setmask_show();
		view[0].setmask_show();
		view[1].setmask_show();
	    }

	    view[1].ChangeView(view[0]);
	    view[0].reproject();
	    view[1].reproject();
	    cvSet(creation->w, CV_RGB(255,255,255));
	    reader.cloneimages_color();
	    dm.DrawModel3D(view[0], reader.get_colorimage(0),NULL,creation->select);
	    dm.DrawModel3D(view[1], reader.get_colorimage(1),NULL,creation->select);
	    dm.DrawModel3D(model, creation->modelview, creation->w, NULL, creation->select);
	    cvShowImage(window[0],reader.get_colorimage(0));
	    cvShowImage(window[1],reader.get_colorimage(1));
	    cvShowImage(window[2], creation->w);
	}
	draw = 1;
	key = cvWaitKey(0);
	/* teclas:
	   0xXXXX2X/3X -> Letra mayúscula
	   0xXXFF51 -> left, 52->up, 53->right, 54->down
	   0x01XXXX -> shift mayus
	   0x02XXXX -> Mayus
	   0x04XXXX -> control
	   0x08XXXX -> Alt
	   0x10XXXX -> Num Block on
	*/
	key2 = key & (0x0FFFFF ^ 0x020000);
	switch(key2)
	{
	case 0x0ff51: //left
	case 0x0ff96: //left numérico
	    creation->modelview.rotate(0,increment2, 0);
	    break;
	case 0x0ff52: //up
	case 0x0ff97: //up numérico
	    creation->modelview.rotate(-increment2, 0);
	    break;
	case 0x0ff53: //right
	case 0x0ff98: //right numérico
	    creation->modelview.rotate(0,-increment2, 0);
	    break;
	case 0x0ff54: //down
	case 0x0ff99: //down numérico
	    creation->modelview.rotate(increment2, 0);
	    break;
	case '0': 
	    creation->modelview.resetR();
	    break;
	case 's':   //Salvar el modelo a un archivo
	case 'S':
	    //almacena las matrices de puntos2D y 3D
	    SaveModel(basefilename, reader.get_ROI());
	    break;
	case 'h':
	case 'H':
	    hide_points = 1-hide_points;
	    printf("hide_points is %d\n", hide_points);
	    break;
	case 'd':
	case 'D':
	    // delete point
	    model.deletepoint(creation->select);
	    view[0].deletepoint(creation->select);
	    view[1].deletepoint(creation->select);
	    creation->modelview.deletepoint(creation->select);
		
	    printf("Point %d deleted\n", creation->select);
	    if (creation->select >= model.npoints)
		creation->select--;
	    model.calcLinesModel();
	    break;
	case 'a':
	case 'A':
	    //add a point at the centre of the view
	    creation->modelview.addpoint();
	    view[0].addpoint();
	    view[1].addpoint();
	    creation->select = model.addpoint(view[0].R);

	    printf("Point %d added\n", creation->select);

	    ang = cvmGet(view[0].R, 1,0) *180.0/CV_PI;
	    if (ang > 15)
		ang += 15;
	    else if (ang < -15)
		ang -= 15;

	    difang = 25.0;
	    cvmSet(model.angY, creation->select, 0, (ang +difang) *CV_PI/180.0);
	    cvmSet(model.angY, creation->select, 1, (ang -difang) *CV_PI/180.0);

	    model.calcLinesModel();
	    break;
	case '1':
	    break;
	case '2':
	    break;

	    //SHIFT + ...
	case  0x1FF51: //left
	    view[0].rotate(0,0,-increment);
	    break;
	case  0x1FF53: //right
	    view[0].rotate(0,0,increment);
	    break;

	    //SHIFT
	case 0x1FF52: //UP
	    view[0].translate(0,0,-incrementpos);
	    break;
	case 0x1FF54: //DOWN
	    view[0].translate(0,0,incrementpos);
	    break;

	    //ctrl + ...
	case  0x4FF51: //left
	    view[0].rotate(0,increment);
	    break;
	case  0x4FF53: //right
	    view[0].rotate(0,-increment);
	    break;
	case 0x4FF52: //UP
	    view[0].rotate(-increment);
	    break;
	case 0x4FF54: //DOWN
	    view[0].rotate(increment);
	    break;

	    //ALT + ...
	case  0x8FF51: //left
	    view[0].translate(-incrementpos);
	    break;
	case  0x8FF53: //right
	    view[0].translate(incrementpos);
	    break;
	case 0x8FF52: //UP
	    view[0].translate(0,-incrementpos);
	    break;
	case 0x8FF54: //DOWN
	    view[0].translate(0,incrementpos);
	    break;
	case '+':
	    cvmSet(model.angY, creation->select, 0, creation->modelview.Ry()-0.01);
	    printf("piont %2d ángulos ( %.0f <- -> %.0f )  \n", creation->select,  
		   (cvmGet(model.angY, creation->select, 0) * 180.0/CV_PI), 
		   (cvmGet(model.angY, creation->select, 1) * 180.0/CV_PI));
	    break;
	case '-':
	    cvmSet(model.angY, creation->select, 1, creation->modelview.Ry()+0.01);
	    printf("piont %2d ángulos ( %.0f <- -> %.0f )  \n", creation->select,  
		   (cvmGet(model.angY, creation->select, 0) * 180.0/CV_PI), 
		   (cvmGet(model.angY, creation->select, 1) * 180.0/CV_PI));
	    break;
	default:
	    draw=0;
	}
	if (key2 >= '1' && key2 <= '3')
	{
	    //assign threshold for inliers/ourliers comparation
	    int t = key2 - '1';
	    cvmSet(model.thresholds2, creation->select, 0, model.umbral_dist2[t]);
	    draw = 1;
	}
    } while (key2 != 27 && key2 != 'Q' && key2 != 'q' && key2 != ' ');

}



void on_mouse0(int event, int x, int y, int flags, void* param)
{
    //printf("event:%X, flags:%X\n", event, flags);
    //return;
    
    /*
      Flags:
      --------------------
      0x01 -> left button
      0x02 -> right button
      0x04 -> center button
      0x08 -> control
      0x10 -> shit mayus
      0x20 -> alt or Bloq Num

      Events:
      --------------------
      1/4 -> press/release left button
      2/5 -> press/release right button
      3/6 -> press/release center button

    */

    if (event != 1) // && flags == 0) 
	return;

    CFacedir *p = (CFacedir *)param;    
    p->on_mouse(event, x, y, flags, 0);
}


void on_mouse1(int event, int x, int y, int flags, void* param)
{
    if (event != 1) // && flags == 0) 
	return;

    CFacedir *p = (CFacedir *)param;    
    p->on_mouse(event, x, y, flags, 1);
}


void CFacedir::on_mouse(int event, int x, int y, int flags, int iview)
{
    float x1, x2, y1, y2;

    reader.cloneimages_color();

    // select the closer point to the mouse click in the image plane
    if (event == 1 && (flags&(0x20^0xff)) != 8) //click
    {
	creation->select = view[iview].closer_point(x,y);
	
	printf("piont %2d. Ángulos ( %.0f <- -> %.0f ). Umbral %3.1f  \n", creation->select,  
	       (cvmGet(model.angY, creation->select, 0) * 180.0/CV_PI), 
	       (cvmGet(model.angY, creation->select, 1) * 180.0/CV_PI),
	       sqrt(cvmGet(model.thresholds2, creation->select, 0)));
    }
    else   //ctrl + click
    {
	float ur, vr, ul, vl;

	if (iview == 1)
	{
	    vr = -cvmGet(model.epilines, creation->select, 2)/cvmGet(model.epilines, creation->select, 1);
	    vr += -cvmGet(model.epilines, creation->select, 0)/cvmGet(model.epilines, creation->select, 1)*(float)x;
	    y = (int)vr;
	}
	cvmSet(view[iview].points2d, creation->select, 0, x);
	cvmSet(view[iview].points2d, creation->select, 1, y);

	//obtención del punto 3D correspondiente
	ul = (float)cvmGet(view[0].points2d, creation->select, 0);
	vl = (float)cvmGet(view[0].points2d, creation->select, 1);
	ur = (float)cvmGet(view[1].points2d, creation->select, 0);
	vr = (float)cvmGet(view[1].points2d, creation->select, 1);

	CvMat *XYZi = cvCreateMat(3,1,CV_32FC1);
	//obtiene el vector T del punto, con respecto al mundo
	GetStereoPoint(cvPoint2D32f(ul, vl), cvPoint2D32f(ur, vr), 
		       view[0].calibration, 
		       view[1].calibration, 
		       XYZi);

	// se pasa T a coordenadas del modelo restandole offsetmodel.
	//el desplazamiento del modelo con respecto a su posición inicial es offsettrans
	// y su rotación es vect
	//se resta el desplazamiento del modelo al punto XYZ en coordenadas del modelo
	cvSub(XYZi, view[0].translation, XYZi);

	//se aplica la rotación contraria a vect
	//CvMat points3d2;
	//cvGetRow(points3d, &points3d2, creation->select);
	//la operacion a realizar es XYZi = (-R3x3)*XYZi = rodrigues(-R)*XYZi
	//rodrigues(R) = rodrigues(-R)'  -> 
	//XYZi = rodrigues(-R)*XYZi =  
	//cvGEMM(view[0].rodrigues(), 1, XYZi, 1, NULL, 1, XYZi, CV_GEMM_B_T);
	cvMatMulAdd(view[0].Rneg(), XYZi, NULL, XYZi);
	
	cvmSet(model.points3d, creation->select, 0, cvmGet(XYZi, 0, 0));
	cvmSet(model.points3d, creation->select, 1, cvmGet(XYZi, 1, 0));
	cvmSet(model.points3d, creation->select, 2, cvmGet(XYZi, 2, 0));
	cvReleaseMat(&XYZi);

	if (iview == 0)
	{
	    CvMat pointsI_row, epilines_row;
	    cvGetRow(view[0].points2d,&pointsI_row, creation->select); 
	    cvGetRow(model.epilines,&epilines_row, creation->select); 
	    cvComputeCorrespondEpilines2(&pointsI_row, 1, view[0].calibration->Fundamental, 
					 &epilines_row);
	}
	model.calcLinesModel();

    }
    
    //draw the model in white background
    cvSet(creation->w, CV_RGB(255,255,255));
    reader.cloneimages_color();
    dm.DrawModel3D(view[0], reader.get_colorimage(0),NULL,creation->select);
    dm.DrawModel3D(view[1], reader.get_colorimage(1),NULL,creation->select);
    dm.DrawModel3D(model, creation->modelview, creation->w, NULL, creation->select);

    x1 = 0;
    x2 = reader.image_size().width-1;
    y1 = y2 = -cvmGet(model.epilines, creation->select, 2)/cvmGet(model.epilines, creation->select, 1);
    y1 += -cvmGet(model.epilines, creation->select, 0)/cvmGet(model.epilines, creation->select, 1)*x1;
    y2 += -cvmGet(model.epilines, creation->select, 0)/cvmGet(model.epilines, creation->select, 1)*x2;
    
    cvLine(reader.get_colorimage(1), 
	   cvPoint((int)x1, (int)y1), cvPoint((int)x2, (int)y2), CV_RGB(0,0,150), 1, 8, 0);

    cvShowImage(window[0],reader.get_colorimage(0));
    cvShowImage(window[1],reader.get_colorimage(1));
    cvShowImage(window[2], creation->w);

}


